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ABSTRACT 


A  serial-parallel  robotic  manipulator  can  be  viewed  as  a  closed  mechanism  with 
multiple-arms.  It  has  the  characteristics  of  both  configurations:  the  high  stiffness  and 
accuracy  of  a  parallel  robot,  and  a  large  workspace  and  compact  structure  of  a  serial 
robot.  Two  serial-parallel  robotic  manipulators  (a  linkage  robot  and  an  articulated  arm 
platform  robot)  were  studied  in  this  research  on  their  direct  and  inverse  kinematics, 
velocity,  dynamics,  and  collision  detection. 

The  direct  and  inverse  kinematics  problems  were  solved  using  the  wrist  position 
of  each  arm.  The  inverse  velocity  problem  was  solved  by  the  force  analysis  and  the 
principle  of  virtual  work.  Based  on  the  velocity  analysis,  the  singularity  problem  was 
researched,  and  the  redundant  actuation  was  analyzed.  The  dynamic  models  were 
established  using  the  Lagrange  formulation.  An  efficient  algorithm  was  introduced  to 
detect  link  collision.  This  algorithm  can  be  extended  to  multiple  cooperating  robots  as 
well.  MATLAB  was  used  to  simulate  robot  motion  and  to  verify  various  control 
algorithms. 

To  demonstrate  the  mobility  and  capability  of  a  linkage  robot,  a  prototype  was 
built  from  off-the-shelf  components  whenever  possible.  Rhino  robot's  controller  along 
with  its  motors  is  used  to  control  this  linkage  robot. 


CHAPTER  ONE 


INTRODUCTION 


1.1  Background 

Most  industrial  robots  have  a  serial  configuration  that  consists  of  links  serially 
jointed  together.  They  have  a  large  workspace  with  a  compact  structure.  However  their 
positioning  errors  are  significant  because  of  the  flexibility,  friction,  and  backlash  in  each 
joint. 


Parallel  manipulators,  like  those  used  in  flight  simulators,  have  the  advantages  of 
high  stiffness  and  accuracy  because  actuators  act  in  parallel  to  share  a  common  payload, 
and  because  errors  are  distributive  in  parallel  rather  than  accumulative  in  series. 

A  serial-parallel  robot  has  the  characteristics  of  both  configurations:  the  high 
stiffness  and  accuracy  of  a  parallel  robot,  and  a  large  workspace  and  compact  structure  of 
a  serial  robot.  Notash  and  Podhorodeski  [1994]  presented  a  broad  class  of  three 
articulated-arm  parallel  manipulators.  These  manipulators  consist  of  a  mobile  platform 
supported  by  three  arms,  each  comprised  of  three  revolving  joints  with  a  passive  spherical 
joint.  Forward  displacement  solutions  were  presented  for  all  possible  combinations  of 
non-redundant  and  redundant  actuation. 

Wang  [1991]  proposed  a  6-bar  linkage  for  a  5  degree-of-freedom  (DOF)  robot, 
which  can  be  viewed  as  a  platform  robot  with  two  articulated  arms.  The  6th  DOF  can  be 
provided  with  a  roll  motor  at  the  wrist.  The  force  and  motion  control  of  this  robot  was 
discussed  by  You  [1992], 

For  dynamics  analysis  and  torque  control,  Pang  and  Shahinpoor  [1994]  analyzed 
the  inverse  dynamics  of  a  3  DOF  hybrid  parallel  manipulator  using  Lagrange’s  method. 
Baiges  and  Duffy  [1995]  derived  the  explicit  dynamic  equations  of  a  parallel  manipulator 
using  Kane's  method.  The  explicit  dynamic  equations  allowed  the  designer  to  examine 
the  effects  of  different  factors  such  as  link  dimensions. 

A  serial-parallel  robot  has  two  or  more  serial  arms,  and  the  collision  of  links  is 
possible.  For  collision  detection  of  multiple  robots,  Hurteau  and  Stewart  [1988]  proposed 
a  minimum  distance  algorithm  for  imminent  collision  indication  in  an  off-line  graphical 
robot  simulation  system.  The  algorithm  is  based  on  the  minimum  distance  between  pairs 
of  convex  polyhedral  objects.  Chang  [1990]  used  minimum  distance  functions  for 
planning  collision-free  motion  of  two  articulated  robot  arms,  and  he  modeled  links  as 
cylinders  and  spheres.  Cao  [1993]  modeled  links  the  same  way  and  presented  a  fast 
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collision  detection  and  avoidance  algorithm  by  using  Lagrange  multipliers  to  find  the 
minimum  distance  between  two  links. 

Amirouche  and  Jia  [1988]  presented  a  method  using  differential  geometry  and 
body  vectors  to  establish  norms  and  planes  to  formulate  the  geometrical  constraints 
needed  for  multi-robot  collision  avoidance. 


1.2  Scope 

In  Chapter  Two,  the  direct  and  inverse  kinematics  of  the  linkage  and  platform 
robots  are  analyzed  to  relate  the  position  and  orientation  of  the  tool  to  joint  angles.  A 
MATLAB  programs  were  written  to  verify  the  kinematics  and  to  demonstrate  the  motion 
animation  of  these  robots. 

In  Chapter  Three,  a  linkage  robot  with  redundant  actuation  is  presented  to  avoid 
singularities.  Velocity  and  force  analyses  based  on  the  position  Jacobian  matrix  of  the 
wrists  were  performed.  This  approach  is  based  on  serial  robot  models. 

In  Chapter  Four,  velocity  and  force  control  algorithms  of  linkage  and  platform 
robots  are  derived  based  on  parallel  manipulators’  approach  using  screw  theory  and 
wrenches.  Methods  based  on  serial  robots  and  parallel  robots  should  generate  the  same 
results.  MATLAB  programs  are  coded  to  verify  this  hypothesis. 

In  Chapter  Five,  the  linkage  robot’s  dynamic  model  is  established  using  the 
Lagrange  formulation  to  relate  the  actuator  torque  to  the  links’  motion.  Equations  of 
motion  for  a  three  articulated-arm  robot  are  derived  similarly. 

In  Chapter  Six,  an  efficient  link  collision  detection  algorithm  for  linkage  and 
platform  robots  is  presented.  Cylinders  are  used  to  model  links,  and  line  equations  with 
parameters  are  used  to  represent  center-lines  of  these  cylinders.  If  the  distance  between 
two  center-lines  is  less  than  the  sum  of  radii  of  these  two  cylinders,  and  the  feet  of  the 
common  normal  of  the  two  center-lines  are  inside  the  links,  collision  happens.  MATLAB 
programs  are  written  to  simulate  the  motion  and  detect  link  collision. 
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CHAPTER  TWO 


KINEMATICS  OF  LINKAGE  AND  PLATFORM  ROBOTS 

A  serial  robot  manipulator  can  be  modeled  as  a  chain  of  rigid  bodies  connected  by 
joints.  In  a  parallel  robot,  the  platform  is  supported  by  linear  actuators  in  parallel.  In  a 
serial-parallel  robot,  the  platform  is  connected  to  the  base  by  multiple  serial  chains.  In 
this  chapter,  the  Denavit-Hartenberg  (D-H)  coordinate  transformation  is  first  reviewed. 
Then  the  direct  and  inverse  kinematics  of  a  linkage  robot  and  a  three  articulated-arm 
platform  robot  are  solved.  Finally,  motion  simulation  programs  were  written  in  MATLAB 
to  simulate  the  straight-line  path  motion  of  the  linkage  and  platform  robots. 

2.1  Denavit-Hartenberg  (D-H)  Coordinate  Transformation 

The  objective  of  the  D-H  coordinate  transformation  is  to  systematically  assign 
coordinate  frames  to  links  and  then  to  transform  position  and  orientation  from  the  tool 
frame  to  the  base  frame.  Coordinate  frame  Lk  will  be  attached  to  the  distal  end  of  link  k. 

The  coordinate  zk_1  is  defined  along  joint  k,  and  xk  is  defined  as  the  common  normal  of 
zk_1  and  zk ,  as  shown  in  Figure  2.1. 
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Figure  2. 1  D-H  parameters 

Let  pk  and  pk_1  be  the  homogeneous  coordinates  of  a  point  with  respect  to  the 
frame  Lk  and  Lk_t  respectively.  The  coordinate  transformation  can  be  represented  by 
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pkl 


where  Tk_j 


Tk-i  PK 

'ce* 

-cctks6k 

sctks6k 

akcQ 

sQk 

cakc6k 

scckcQk 

aksQ 

0 

sak 

CCLk 

dk 

0 

0 

0 

1 

,  and 


$9*,  cQk,  sak,  cak  represent  sin 0*,  cos0*,  since*, 


(2.1) 

(2.2) 


and  cos  a*  respectively. 


The  D-H  parameters  involved  in  (2.2)  are  0* ,  dk,  ak,  and  ak .  The  joint  angle  0* 
is  the  rotation  about  zkl  needed  to  make  axis  xk_1  parallel  to  axis  xk .  The  joint  distance 
dk  is  the  translation  along  zk_1  needed  to  make  the  axis  xkl  intersect  xk .  The  link 
length  ak  is  the  translation  along  xk  needed  to  make  axis  zk_1  intersect  axis  zk .  The 
twist  angle  ak  is  the  rotation  about  xk  needed  to  make  axis  zkl  parallel  to  axis  zk . 


Four  steps  are  involved  in  the  homogeneous  transformation  Tk_, ,  and  are 
summarized  in  Table  2.1. 


Table  2. 1  Coordinate  transforming  from  frame  k  to  frame  k-1 


Steps 

Description 

1 

Rotate  Lk_,  about  zk_1  by  0* 

2 

Translate  Lk_,  along  zk_1  by  dk 

3 

Translate  Lk_,  along  xk  by  ak 

4 

Rotate  Lk_,  about  xk  by  ak 

The  transformation  matrix  (2.2)  can  be  written  as 


(2.3) 


where  R  and  p  are  the  orientation  and  position  of  frame  Lk  relative  to  frame  Lk_j ,  R  is  a 
3x3  rotation  matrix,  and  p  is  the  translation  vector  from  the  origin  of  the  frame  Lk_,  to 
that  of  frame  Lk . 


2.2  The  Kinematics  of  a  Linkage  Robot 

Figure  2.2  shows  the  linkage  structure  of  a  serial-parallel  robot  [Wang  1991]. 
Because  the  linkage  robot  has  5  DOF,  only  five  joints  need  to  be  active.  We  can  locate 
the  active  joints  close  to  the  base  to  reduce  the  moving  inertia.  Thus,  joints  LI ,  L2 ,  L}, 
R, ,  and  R}  are  chosen  as  being  active.  By  controlling  these  five  joints,  the  tool's  position 
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as  well  as  pitch  and  yaw  angles  can  be  controlled.  The  roll  angle  of  the  tool  can  not  be 
controlled,  however. 


Figure  2.3  The  left  and  right  arms  of  a  linkage  robot 
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To  simplify  the  analysis  of  this  linkage  robot,  we  can  view  it  as  two  connected 
serial  arms  as  shown  in  Figure  2.3.  The  left  arm  has  a  2  DOF  shoulder,  an  elbow  and  a 
spherical  wrist  joint.  The  right  arm  has  a  similar  configuration  except  that  its  wrist  has 
only  2  DOF. 

The  coordinate  frames  of  the  linkage  robot  are  shown  in  Figure  2.4.  The  dashed 
lines  indicate  the  coincident  origins  of  L0and  L,,  L2and  L3,  and  L4and  Ls 
respectively.  The  D-H  parameters  are  listed  in  Table  2.2. 


Figure  2.4  Coordinate  frames  of  a  linkage  robot 


Table  2.2  Kinematics  parameters  of  the  linkage  robot 


Axis 

e, 

d. 

1 

a, 

a , 

Home 

1 

<1, 

0 

0 

71/2 

0 

2 

0 

a2 

0 

7t  /  2 

3 

0 

0 

— 7t  /  2 

-n/2 

4 

<h 

d4 

0 

n/2 

0 

5 

4s 

0 

0 

-71/2 

n/2 

6 

<?6 

d(, 

0 

0 

0 

The  transformation  matrix  of  the  left  arm  from  the  frame  4  to  the  frame  0  can  be 
expressed  as: 


T4  = 
Ao 


CL\ 

0 

SLl 

o' 

CL2 

■^2,1 

0 

a2CL2 

CO 

-J 

o 

1 _ 

0 

SL3 

o' 

C24 

0 

SL4 

o' 

SL\ 

0 

~  CLl 

0 

SL2 

CL\ 

0 

a2SL2 

SL3 

0 

CL3 

0 

h4 

0 

—  CL4 

0 

0 

1 

0 

0 

0 

0 

1 

0 

0 

-1 

0 

0 

0 

1 

0 

^4 

0 

0 

0 

1 

0 

0 

0 

1 

0 

0 

0 

1 

0 

0 

0 

1 

CUCL23CL4  ~SLlSL4  ~CL\SL4  CL\CL23SL4  +  SUCL4  °2C L\C L2  ~  ^ 4C L\S L23 

SL\CL23CL4  +  CL\SL4  ~  SL\SL4  SL\CL23SL4  ~  C L\C L4  a2SLlCL2  ~  ^4SL\SL23 

SL23  CL23  SL23SL4  a2SL2  +  ^4CL23 

1 


0  0  0 
where  cu  is  cos 0O,  and  SL23  is  sin(0L2  +  0t3). 


(2.4) 


The  inverse  kinematics  problem  is  to  find  joint  angles  given  the  position  vector  p 
and  orientation  matrix  R  =  [n  o  a] ,  where  n,  o,  a  are  defined  as  the  normal, 
orienting,  and  approach  vectors,  and  are  the  principal  axes  of  the  tool  coordinate  frame. 
For  a  5  DOF  robot,  only  the  hand’s  position  p  and  approach  vector  a  can  be  specified. 

The  position  of  the  left  and  right  wrists  can  be  expressed  in  terms  of  p  and  a  as: 

Pl=P  “4a  (2.5) 

Pr  =P-4a  (2.6) 

where  lL  and  lR  are  the  distances  from  the  tool  to  the  left  and  right  wrist  respectively. 

From  the  p  in  T04  using  (2.3),  the  position  of  the  left  wrist  can  be  expressed  as: 


P  Lx  a2CL\CL2  ^4CL\SL23 
Ply  ~a2SL\CL2  ~^4SL\SL23 
Plz  =  a2SL2  ~^^4CL23 


(2.7) 

(2.8) 
(2.9) 


Solving  these  equations,  the  active  joint  angles  can  be  obtained  as: 

-1  Ply 

Plx 


0L1  =  tan 


0L3  =  sin 


.  .X+^-pL-pL 


■pL 


JL2 


2d4a2 


tan 


Pu(a2  ^4SL3)  ^4CL3(PlxCLI  PLySLl) 
PlACL3  +  (a2  -  d4SL3)(PuCLl  +  PLySLl ) 


(2.10) 

(2.11) 

(2.12) 


The  right  arm’s  D-H  parameters  are  similar  to  those  of  the  left  arm,  and  its  joint 
angles  can  be  solved  as: 
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(2.13) 


®R1 


=  tan 


PRy 

PRx-h 


e 

0 


R3 


R2 


.  _i  a2  +cl4  - (pRx  - h)  -pRy-pRz 
sm  - ~ - 

2d4a2 

_1  PRz(a2  —  *^4SR3)  —  ^4CR3t(PRx  —  ^)CR,  +  pRySR|] 
tan  - 

Prz^4^R3  +  (a2  “  ^4SR3)[(Prx  “  ^)CR1  +  PRySRl)] 


(2.14) 

(2.15) 


The  direct  kinematics  is  to  find  the  hand  position  and  orientation  given  the  active 
joint  positions.  The  position  of  the  right  wrist  is  a  function  of  0R1 , 0R2 ,  and  0fi3 ,  similar 
to  (2.6)  to  (2.8),  where  0R1  and  0R3  are  given,  and  only  0R2  is  unknown.  The  distance 
between  the  two  wrists  is  a  constant  k  and  can  be  expressed  as  a  function  of  the  joint 
angles,  including  the  unknown  0R2 ,  as  shown 

^  =  \pu  —  Prx(®R2)]  +  \pLY  —  +  \pLZ  ~  Prz(®R2)] 

The  angle  0R2  can  then  be  obtained  by  solving  this  equation.  Once  0R2  is  known,  the 
position  of  the  right  wrist  can  be  found  using  a  numerical  method.  Then  the  approach 
vector  a  can  be  expressed  as: 


a  Pr-Pl 

k-Pi. 


(2.16) 


The  position  of  the  end  effector  can  then  be  obtained  in  (2.4).  In  addition  to  a,  the 
other  two  vectors  n  and  o  of  the  orientation  matrix  R  need  to  be  decided.  Unit  vector  b, 
which  is  in  the  direction  of  the  right  forearm,  can  first  be  decided  by  the  positions  of  the 
right  elbow  and  wrist.  Once  b  is  known,  orientation  vector  o  can  be  obtained  as 
o  =  a  x  b ,  and  then  normal  vector  n  can  be  obtained  as  n  =  a  x  o . 


2.3  The  Kinematics  of  a  Platform  Robot 

Because  the  platform  robot  has  6  DOF,  only  six  out  of  all  18  joints  need  to  be 
active.  Therefore,  only  two  joints  of  each  arm  are  chosen  as  active,  similar  to  the  right 
arm  of  the  linkage  robot.  By  controlling  these  six  joints,  the  platform’s  position  and  its 
orientation  can  be  controlled. 

The  coordinate  transformation  from  a  position  vector  pp  of  the  platform 

coordinates  to  that  of  the  base  coordinates  p  can  be  expressed  as: 

P  =  RPp+Po  (2-17) 

The  feet  on  the  base  and  the  platform  constitute  an  equilateral  triangle 
respectively,  as  shown  in  Figure  2.5.  The  origin  of  the  global  frame  is  at  the  base 
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triangle’s  center,  and  that  of  the  tool  frame  is  at  the  platform  triangle’s  center.  The  matrix 
R  and  vector  p0  represent  the  orientation  and  translation  of  the  platform  vector  relative  to 
the  base.  The  feet  on  the  base  in  the  global  coordinate  frame  can  be  expressed  as: 

b  =  [rhchi,  rbsbi,  o]  (2.18) 


Figure  2.6  One  arm  of  the  platform  robot 
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The  coordinates  of  the  feet  on  the  platform  in  the  platform  coordinate  frame  can 
be  expressed  as: 

Pf=| rPcpi’  rpsPn  °]  (2.19) 

where  rb  and  rp  are  the  radii  of  the  circles  of  the  feet  on  the  base  and  platform 
respectively.  Using  (2.17),  the  global  coordinates  p,  can  be  obtained  as: 

Pi  =  Rp?  +  Po  (2.20) 


Each  arm  of  the  platform  robot  can  be  treated  like  a  serial  arm,  as  shown  in  figure 
2.6.  The  transformation  matrix  from  the  wrist  on  the  platform  to  the  foot  on  the  base  can 
be  expressed  similar  to  (2.5)  as: 


I  rp4  _ 

A0  “ 


Ci\Ci23Ci4  SilSi4 


_<71  ^<23  ^il^/23^/4  <U<'iiCi2  ^4Ci\Si23 


Si\Ci23Ci4  ^  CilSi4  CilSi23 


S: 


723 

0 


Ci23 

0 


snci23s<4  cnci4 
Si23Si4 


0 


a2SilCi2  ^4Si\Si23 
+ 

1 


ai2Si2  Jr(^4Ci23 


(2.21) 


The  position  vector  from  a  foot  on  the  base  to  a  corresponding  wrist  on  the 
platform  is  equal  to  the  position  vector  in  the  matrix  (2.21)  as: 

a2CnCi2  ~^4CnSi23 

1  (2.22) 


Pi -bj  = 


a2Si\Ci2 


-  d^s^Sj 


23 


a2S2  ^4Ci23 


The  joint  angles  can  then  be  solved  similar  to  (2.10)-(2.12). 

2.4  The  Prototype  Robot 

To  demonstrate  the  mobility  and  capability  of  this  linkage  robot,  a  prototype  was 
built  as  shown  in  Figure  2.7.  To  simplify  the  construction,  off-the-shelf  components  are 
used  whenever  possible.  Universal  joints  are  commercially  available  but  not  used  for 
wrists  because  their  motion  range  is  limited.  An  in-line  pitch  joint  along  with  two  roll 
joints  is  designed  to  constitute  a  spherical  wrist.  The  in-line  pitch  joint  allows  the  pitch 
motion  of  up  to  75  degrees,  and  the  stiffness  and  strength  of  the  tongue  and  yoke  of  this 
joint  are  preserved  by  using  v-shaped  ribs,  as  shown  in  Fig.  2.8.  The  simplicity  of  this  in¬ 
line  pitch  joint’s  geometry  ensures  a  simple  control  algorithm. 

Selecting  a  robot  controller  at  an  affordable  price  and  with  minimum 
modifications  is  a  challenging  issue.  Rhino  robot's  controller  along  with  its  motors  were 
selected  to  control  this  linkage  robot  because  the  Rhino  robot  is  a  low-cost  educational 
robot  with  extensive  documentation.  Because  servo  gearmotors  are  used,  the  robot  is  not 
backdrivable  and  brakes  are  not  needed. 
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Figure  2.7  A  prototype  linkage  robot 


Figure  2.8  The  in-line  pitch  joint 

Figure  2.9  shows  the  Rhino  robot  in  the  background  sitting  on  top  of  the 
controller.  The  teach  pendant  is  on  the  table  top  and  on  the  left.  The  prototype  robot  is  on 
the  front,  sharing  the  controller  and  teaching  pendant  of  the  Rhino  robot. 
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Figure  2.9  The  Rhino  robot,  prototype  robot,  controller  and  teach  pendant 


The  controller  receives  input  commands  from  a  teach  pendant  or  an  off-line 
programming  environment  ROBOT ALK  [Rhino  Robots  1992]  on  an  IBM  PC. 
ROBOT ALK  can  not  be  used  for  the  linkage  robot  because  it  is  designed  for  the  Rhino 
robot’s  geometry.  The  motion  control  programming  is  feasible  because  low  level  control 
commands  are  provided  [Schilling  and  White  1990],  and  programs  written  in  C  or  BASIC 
can  be  downloaded  from  a  PC  to  the  controller  through  a  serial  port. 


2.5  Motion  Simulation  Programs 

MATLAB  programs  were  written  to  simulate  the  straight-line  trajectory  motion  of 
the  linkage  and  platform  robots.  The  user  can  specify  the  starting  and  ending  position,  and 
the  orientation  of  the  tool.  The  simulation  programs  use  knot  points  and  point-to-point 
control  to  approximate  a  straight  line  motion. 

Figure  2.10  shows  a  screen  of  the  platform  robot’s  simulation.  The  user  can  also  change 
the  eye  position  to  view  the  simulation  in  different  directions.  The  “start”  button  is  to 
start  the  simulation,  and  the  “info”  gives  the  help  information. 


Figure  2.10  An  example  of  the  motion  simulation  program 
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CHAPTER  THREE 


MOTION  AND  FORCE  CONTROL  WITH  REDUNDANT  ACTUATION 

As  discussed  in  Chapter  Two,  five  motors  are  needed  to  control  the  5  DOF  motion 
of  the  linkage  robot.  These  five  motors  should  be  located  close  to  the  base  to  reduce 
moving  inertia.  If  all  major  axes  except  the  right  elbow  are  active,  these  five  motors  are 
insufficient  to  deliver  the  five  intended  freedoms  of  the  linkage  robot  in  some 
configurations,  and  this  can  be  illustrated  with  a  force  analysis. 

Figure  3.1  shows  a  free-body  diagram  of  the  robot.  Since  the  two  wrists  are 
passive,  only  reaction  forces  are  present  to  balance  the  external  load.  Force  fL  at  the  left 
wrist  has  three  components  controlled  by  joint  torques  in  the  left  arm,  xu,  ti2;  and  xL3 . 
Each  force  is  along  an  axis  reciprocal  to  the  other  two  joint  axes  in  the  same  arm  [Notash 
and  Podhorodeski  1994],  fLl  js  provided  by  xL1,  and  is  in  the  direction  parallel  to  the 
second  and  the  third  joints.  Therefore  fL2  is  provided  by  xL2,  and  is  the  force  in  the 
direction  of  link  L3.  fL3  is  provided  by  xL3 ,  and  is  the  force  in  the  direction  from  the 
left  shoulder  to  the  left  wrist.  Force  fR  at  the  right  wrist  has  two  components,  fRX  and 
fRZ ,  defined  similar  to  those  at  the  left  wrist. 


Figure  3.1  The  free  body  diagram  of  a  linkage  robot 
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In  Figure  3.2,  the  right  forearm  is  at  a  horizontal  position.  The  two  reaction  forces 
at  the  right  wrist  will  be  on  a  horizontal  plane,  and  therefore  the  coupler-arm  can  not  be 
properly  supported  if  an  external  moment  is  in  the  vertical  plane.  In  general,  since  the 
right  wrist's  force  has  only  two  components,  a  torque  vector  normal  to  the  plane  defined 
by  these  two  forces  can  not  be  supported.  Therefore,  a  linkage  robot  with  five  motors  can 
have  only  four  degrees  of  freedom.  In  order  to  become  a  five  DOF  robot,  the  right  elbow 
must  also  be  actuated  as  shown  in  Figure  3.3. 


Figure  3.2  A  linkage  robot  at  an  immobile  position 


Figure  3.3  A  linkage  robot  with  redundant  actuation 
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3.1  Rates  Control  Algorithm 


The  velocity  analysis  is  to  relate  the  joint  rate  to  the  hand  velocity.  We  can  first 
relate  the  left  wrist's  velocity  vLw  to  the  hand's  linear  and  angular  velocity  v  and  w  as: 
vlw  =  v- wx/La  (3.1) 

As  in  (2.7)  to  (2.9),  the  left  wrist's  position  can  be  expressed  as  functions  of  joint  angles 
ez.i>  >  and  0L3. 

P  Lx  ~a2^ Ll^L2  ~  ^3^L\Sl23 

Ply  ~  a2SL\CL2  ~  ^3SLlSL23  (3.2) 

P Lz  ~  a2SL2  +  hCL23 

Taking  time  derivatives  of  (3.2),  we  have: 

VLw  _  JLp^Lp  (3.3) 

where  qLp  —  [^lp0^0^]  »  and  JLp  is  a  3x3  position  Jacobian  matrix. 


^Plx 

dPu 

dp  Lx 

50  LI 

aet2 

30  L3 

dPLy 

dpLy 

dPLy 

38t, 

90  L2 

90^3 

•>Pl, 

dpLz 

dpLz 

_00li 

36 1! 

00  L3. 

SL\  ( a2 

CL2  ~  ^4 

^23) 

CL\(a2 

CL2  —  ^4 

^23) 

1  w4l'/,23  /  “4JL1C£2: 

a2CL2~^4SL23  ~^4CL23 


Inverting  (3.3),  the  joint  rates  can  be  related  to  the  wrist  velocity  as: 

^Lp  =  ^LpVLw 


Similarly,  we  can  relate  the  right  joint  rates  to  the  right  wrist's  velocity  as: 
<Irp=Jrpvrw  (3.6) 


Combining  (3.5)  and  (3.6),  we  have: 
q  =  JP‘vw 

where  q^0^,0^,0^0^0^.0^ ,  vw  = 


,  and  JD  = 


»lp  « 

0  J, 
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si\(hci2+hcm)  cn^isi2+hsm)  hci\sm 

Jjp  —  Cil  (4C/2  hci2‘})  ~Sil(hSi2  +  hsi23^  ~hSilSi23  > 


4C/2  "*■  4C/23 


4  Cj  23 


We  can  rewrite  (3.1)  to  replace  the  cross  product  by  a  matrix-vector  multiplication 
as: 

vlw=v  +  4Aw  (3.7) 

0  a, 

where  A  =  az  0  ,  and  ax,ay,az  are  components  of  vector  a . 

ray  a*  0  . 

Similar  to  (3.7),  we  can  have: 

vrw=v  +  4Aw  (3.8) 


Combining  (3.7)  and  (3.8),  we  have: 


vw  =Bj 


fl  d,  A 
where  Bj  =  ,  4 


I  ^A 


(3.10) 


Substituting  (3.9)  into  (3.6),  we  have: 


q=J;1B1 


(3.11) 


Matrix  B,  is  singular  because  in  (3.9),  the  wrist  velocity  is  independent  of  the 


hand's  roll  velocity  coa .  Therefore  only  five  components  of  the  hand  velocity  are 

|_w 

mapped  to  a  six-component  wrist  velocity  Vw ,  and  B,  is  not  full  rank.  To  exclude  toa  in 
the  velocity  analysis,  we  can  relate  w  the  angular  velocity  in  the  tool  coordinate  frame  to 
that  in  the  Cartesian  coordinate  frame  wt  =  [con,coo,coa]r  as: 


w  =  Rw, 


(3.12) 


where  w  =  coy  ,  and  w,  =  coo 


Substituting  (3.12)  into  (3.7)  and  (3.8),  we  can  have: 
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3.2  Force  Control  Algorithm 

The  force  analysis  is  to  relate  the  joint  torque  t  =  [TL1,Tt2,Ti3,TR1,TR2,TR3]  to 

[F]  _ 

the  external  load  ^  =  [Fx,Fy,Fz,Tx,Ty,Tz]  .  Using  the  principle  of  virtual  work  [Paul 

1981],  we  can  relate  the  joint  torque  to  the  wrist  force  by  the  same  Jp  defined  in  (3.5)  as: 
t  =  JpTfw  (3.16) 
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where  t  is  the  joint  torque,  and  fw  = 


lLw 


lRw 


is  the  wrist  force  in  the  Cartesian  coordinates, 


not  fu  ’fu  R3  defined  in  the  previous  section.  To  relate  the  external  load 

wrist  force,  we  can  perform  a  force  and  moment  equilibrium  as: 

F  =  Xf„ 

T  =  IrtaxfLw 

Combining  these  two  equations,  we  have: 

F  i  _ 

Bfr, 


to  the 


(3.17) 


where  Bj  is  defined  in  (3.10).  Since  torque  about  the  coupler  arm  Ta  can  not  be 

rr 

supported  by  f  w ,  we  can  use  a  five  DOF  payload  T  ,  where  the  subscript  t  indicates 

L  J5 

the  torques  in  the  tool  coordinate  frame,  and  F  has  3  components  expressed  in  the 

Z 

Equation  (3.17)  can  then  be  rewritten  as: 


Cartesian  coordinate  frame,  and  Tt  = 

=  BTf 


T 

L  V 


F 

Tt 


(3.18) 


Because  B  is  not  a  square  matrix,  it  can  not  be  inverted.  The  pseudo-inverse 
[Nakamura  1991]  will  be  used  to  relate  the  wrist  force  to  the  external  load. 

F 

(3.19) 


fw  =  B(B  B) 


-1 


T, 


Substituting  (3.19)  into  (3.17),  we  obtain  the  resolved  force  control  as: 
t  =  JDTB(BTB)  1  F 


(3.20) 


Notice  that  the  pseudo-inverse  solution  produces  a  minimum  norm  least-squares 
solution  [Nakamura  1991]  of  the  wrist  force  in  (3.19),  and  in  turn  produces  a  minimum 
norm  least-squares  solution  of  the  joint  torque  in  (3.20). 
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CHAPTER  FOUR 


FORCE  AND  MOTION  CONTROL  BASED  ON  WRENCHES 

The  resolved  force  control  algorithm  based  on  wrenches  is  derived  in  this  chapter 
for  serial-parallel  robots,  the  three  articulated-arm  platform  robot,  and  the  linkage  robot. 
The  resolved  rate  control  algorithm  is  derived  from  the  force  control  algorithm  based  on 
the  principle  of  virtual  work. 

Wang  [1994]  used  the  wrenches  of  screw  theory  [Hunt  1978]  to  derive  a  force 
control  algorithm  for  a  full  parallel  robot.  This  approach  is  dual  to  the  velocity  control 
approach  discussed  in  Fichter  [1986].  The  force  control  approach  is  the  better  approach 
for  serial-parallel  robots  because  redundant  information  about  passive  joint  rates  is  not 
required.  MATLAB  is  used  to  verify  these  control  algorithms.  Graphic  animation  with 
stick  models  of  robots  is  also  accomplished  with  these  programs. 


4.1  A  Three  Articulated-arm  Platform  Robot 


As  discussed  in  Chapter  Two,  only  the  base  and  elbow  joints  of  each  arm  of  the 
platform  robot  need  to  be  active.  Each  active  joint  controls  a  corresponding  wrist  force  on 
the  platform. 

Figure  4. 1  shows  a  free-body  diagram  of  the  platform  robot.  Each  wrist  force  has 
two  components  f„  and  fi3 ,  where  i  indicates  the  wrist  number  or  the  arm  number,  and 

the  second  subscript  indicates  the  active  joint.  Each  fy  passes  through  the  wrist  i  and  is 

along  a  line  reciprocal  to  the  other  two  major  joint  axes  in  the  same  arm,  as  discussed  in 
Chapter  Two. 


The  force  analysis  is  to  relate  the  joint  torque  to  the 

:  [FxiFy’FZ’Tx,Ty,Tz]T .  The  first  step  is  to  relate  the  external  force  F 


external  load 

to  the  wrist  force  f  =  [fn>  f 21^  f 23'’ f 31^/33]  using  the  force  equilibrium: 


F=I  Ify=I  IUy4 

i= 1 7=1.3  J  /=!  7=1,3  J  7 


where  Uy  is  the  unit  vector  in  the  direction  of  the  wrist  force.  uu  = 


(4.1) 


S:, 


0 


,  and 
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and 


Cil(a2Ci2  ~  ^4Si23  ) 

1 

1 

jr* 

J* 

K> 

_ 1 

Si\(a2Ci2  “  d4S( ?23  ) 

If  joint  i2  is  active,  u(2  = 

Si\Si23 

a2Si2+d4Cm 

_  Ci23 

gt  =  ■yjal  +  dl  -  2a2dAsl3  is  the  distance  from  a  respective  shoulder  i  to  its  wrist. 


Figure  4.1  Free  body  diagram  of  a  three  articulated-arm  platform  robot 


The  external  torque  T  can  be  related  to  the  wrist  force  using  moment 
equilibrium: 


T=il  l,xf„ 

i=l  ;= 1,3  J 


(4.2) 


where  1,  is  the  distance  from  each  wrist  to  the  center  of  the  platform.  Combining  (4.1) 
and  (4.2),  we  have: 


"F 

T 


=  Vf 


(4.3) 


Each  column  of  V  is  a  wrench,  representing  the  wrist  force  in  the  screw 


coordinates  as 


uo 

liXUij 


(4.4) 


Inverting  (4.3),  we  can  solve  for  the  wrist  force  as: 


f  =  V1 


(4.5) 
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Joint  torque  ly  can  be  related  to  wrist  force  f0  by  h{J ,  the  distance  from  joint  ij 
to  the  line  containing  the  wrist  force  fy  as  shown  in  Figure  4.2. 

x,7  =  Kfij  (4-6) 


where  hn  -  a2ci2-dAsi2i 


and  hi3  = 


fl2^4C/3 

8i 


(4.7) 


When  joint  i2  is  active,  hi2  =  \a2ci3\ ,  From  (4.5)  and  (4.6)  we  can  get  the  resolved  force 
control  algorithm  relating  joint  torque  to  the  external  load  as: 


t  =  s-' 


F 

T 


(4.8) 


Figure  4.2  Distance  between  an  active  joint  and  a  corresponding  wrist  force 


Each  column  of  S  can  be  represented  by  that  of  V  divided  by  hy  as  : 
uy  thy 

_(li  X«ij)^ij_ 


(4.9) 


Using  the  principle  of  virtual  work  [Paul  1981],  we  can  get  the  resolved  force 
control  algorithm  relating  the  joint  rates  q  =  [e^e^e^e^;^,©^]7  to  the  hand 

velocity  [v,w]T  as: 
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(4.10) 


The  resolved  rate  control  algorithm  can  also  be  derived  using  wrist  velocity  viw . 
We  can  first  relate  the  wrist  velocity  to  the  hand  velocity  as: 

v.w=v-wxli  (4.11) 

The  wrist  velocity  can  be  related  to  the  rate  of  major  joints  in  the  same  arm 
q  =  [0  0.2,0  3]r  by  Jip,  the  position  Jacobian  matrix,  as 


v,w  =  Jipq, 

(4.12) 

Sil(~a2Ci2  +  ^4Si23  ) 

—  cn(a2si2  +  d4ci2 3 ) 

-  d4cnci23 

where  ^ip  =  cn(a2cn  ~d4s!73  ) 

—  sn(a2si2  +  d4ci23 ) 

—  d4siXci  23 

(4.13) 

0 

a2ci2  —  d4si23 

~d4cm 

Inverting  (4.1 1),  the  joint  rates  can  be  related  to  the  wrist  velocity  as: 
q,=J,;'v,w  (4.14) 


At  a  singularity  (or  uncertainty)  configuration  of  a  parallel  robot,  the  external  load 
cannot  be  balanced  by  the  robot,  and  the  robot  will  collapse  ([Gosselin  and  Angeles 
1990],  [Hunt  1978  and  1983],  [Merlet  1989],  [Mohammadi  1993]).  Thus  V  cannot  be 
inverted  at  this  configuration,  and  (4.8)  will  not  yield  a  solution. 

The  robot  is  in  a  singular  position  when  all  three  f(1  are  coplanar  and  parallel. 
This  will  happen  if  the  triangles  of  the  base  and  platform  are  of  the  same  size,  and  when 
the  platform  is  horizontal.  To  avoid  this  singularity  we  can  have  different  sizes  of  the 
platform  and  base. 

The  other  possible  singularity  is  that  all  three  fu  are  coplanar  and  concurrent. 
This  can  only  happen  when  the  size  of  its  platform  and  base  are  identical  and  the  feet  of 
the  platform  are  right  on  top  of  those  of  the  base.  This  singularity  can  also  be  avoided 
with  different  sizes  of  the  platform  and  base. 

Four  wrenches  fu  can  be  dependent  and  cause  singularity.  This  is  possible  if  a 
wrist  force  fu  is  coplanar  with  the  other  three.  If  joint  i'2  (a  shoulder)  is  active,  this  can 

happen  when  the  forearm  is  coplanar  with  a  horizontal  platform,  and  therefore  coplanar 
with  three  fn .  This  is  why  we  need  to  actuate  the  elbow  joints  instead  of  the  shoulder 
joints.  If  the  elbow  is  active,  fi3  is  coplanar  with  the  platform  only  when  the  platform  and 
the  arm  are  both  on  the  ground.  This  singularity  can  be  prevented  by  using  joint  limits  on 
the  shoulder. 
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When  hy  is  zero,  the  corresponding  column  in  S  is  not  defined,  and  (4.9)  cannot 

be  used  to  find  joint  torque.  This  singularity  is  a  serial  singularity:  the  velocity  at  the 
corresponding  wrist  loses  one  freedom  ([Waldron  1985],  [Wang  and  Waldron  1987]).  hn 
is  zero  when  the  wrist  is  on  top  of  the  shoulder,  and  this  is  a  shoulder  singularity  of  a 
serial  robot.  hi2  or  hn  is  zero  when  the  arm  is  fully  stretched  ( s,  =  0 ),  and  this  is  an 
elbow  singularity  of  a  serial  robot.  This  serial  singularity  will  not  cause  problems  in  the 
resolved  force  control  (4.5).  However  because  the  distance  is  zero,  the  joint  torque  is 
zero.  This  means  the  wrist  forces  are  not  supported  by  joint  motors  but  supported  by  the 
structure  or  frame. 


4.2  A  Linkage  Robot 

If  one  of  the  arms  in  the  platform  robot  is  passive,  the  robot  cannot  have  the 
freedom  of  rotation  or  torque  about  the  line  connecting  the  remaining  two  wrists.  If  we 
extend  the  link  connecting  the  two  wrists  as  a  coupler  link,  as  shown  in  Fig.  2.3,  we  have 
the  five  DOF  linkage  robot.  The  sixth  freedom,  roll  motion,  can  be  accomplished  with  a 
roll  motor  attached  at  the  end  of  the  coupler-arm.  In  the  left  arm,  all  three  major  joints  are 
active;  in  the  right  arm,  the  elbow  and  wrist  are  active.  Figure  4.3  shows  the  free-body 
diagram  of  the  robot.  At  the  left  wrist,  there  are  three  force  components  fn ,  f12  and  f13 ; 
at  the  right  wrist,  there  are  two  force  components  f21  and  f23 . 


The  force  analysis  is  to  relate  joint  torques  to  the  external  load.  The  first  step  is  to 
relate  the  external  force  F  to  wrist  force  f5  -[fu,fn-,fn\f2X,f2-i\  using  the  force 
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equilibrium. 


F  =  Z  s  f„  =  I  s  %f„ 

i= 1  ;'=1,3.2  1=1  ;=1.3,2 

The  external  torque  T  can  be  related  to  the  wrist  force  as: 
T  =  i  X  /,(axfSj) 


(4.15) 


(4.16) 


where  /,  is  the  distance  from  each  wrist  to  the  hand,  and  a  is  the  approach  vector,  in  the 
direction  of  the  coupler-arm.  The  normal  and  orienting  components  of  the  torque  T  can 
be  obtained  by  dot  products  as: 

Tn  =  n  •  T  and  T0  =  o  ■  T  (4.17) 


JL  ^  ^  y 

Combining  (4.15),  (4.16)  and  (4.17),  we  can  relate  ^  =  >  the 


5  DOF  external  load,  to  f5  as: 


Fl 

=  V  f 

m  T515 

1 1  C 


(4.18) 


Each  column  of  V5  can  be  obtained  by  substituting  (4. 17)  into  (4. 16)  as  /,n  •  (a  x  uy ) 


•  ( a  x  Uy ) 


Inverting  (4.18),  we  can  get:  f5  =  V5 1 


(4.19) 


Similar  to  (4.8),  we  can  relate  joint  torques  t5  =  [tu,x12,T13;t21,T23]r  to  the  external  load 


t?  -St 


UU  /hij 


Each  column  of  Ss  can  be  represented  by  (n  •  (a  x  u y ) /  toy 

h  ofaxuy)/^ 


(4.20) 


(4.21) 


Using  the  principle  of  virtual  work,  we  can  relate  q5  =  [0„,012,013;021,023]r  the 
joint  rate  to  the  hand  velocity  as: 
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qs=sj 


V 


(4.22) 


w 


Us 


where 


w, 


: [vx,vy,vz;(0„,coo]  is  the  five-component  hand  velocity,  obtained  by 


eliminating  coo ,  the  last  component  of  w, . 


The  velocity  analysis  of  this  linkage  robot  can  also  be  obtained  by  relating  the 
wrist's  velocity  viw  to  the  hand's  linear  and  angular  velocity  v  and  w  as  (3.1).  The  joint 
rates  can  be  related  to  the  wrist  velocity  as  in  (4.14).  This  way  we  will  get  joint  rates  of 
all  major  axes  of  both  arms  which  include  the  passive  joint  rate  022 . 

The  five  wrist  forces  fy  belong  to  a  five-system  of  screws  [Hunt  1978],  The  screw 

system  will  be  degenerated  to  a  four-system  when  the  left  arm  is  fully  stretched.  This  is 
because  f.2  and  fi3  are  collinear,  and  the  robot's  freedom  is  reduced  by  one.  In  the  force 
field,  the  freedom  lost  is  the  torque  in  the  plane  defined  by  the  left  arm  and  the  coupler- 
arm.  In  the  velocity  field,  the  freedom  lost  is  the  velocity  along  the  direction  in  the  left 
arm.  Jp  is  then  singular  and  the  rate  motion  control  algorithm  fails. 


If  the  right  shoulder  is  active  and  the  right  forearm  is  at  a  horizontal  position,  as 
shown  in  Fig.  4.4,  f21 ,  f21  ,  and  f3i  will  be  on  a  horizontal  plane,  and  the  remaining  two 
forces  f12  and  f,3  cannot  support  an  external  moment  in  the  normal  direction.  This  is  why 
the  elbow,  instead  of  the  shoulder,  is  active.  The  singularity  caused  by  the  elbow  joint  can 
be  avoided  by  joint  limits,  the  same  way  as  in  the  platform  robot. 


4.3  Forward  Velocity  Analysis 

The  forward  velocity  analysis  of  the  linkage  robot  is  to  find  the  hand  velocity 
given  the  joint  rates.  This  can  be  accomplished  by  relating  the  velocity  of  the  two  wrists. 
The  right  wrists’  velocity  can  be  obtained  similar  to  the  left  wrist’s  velocity  in  (3.1)  as: 

vrw  =  v  -  lRw  x  a  (4.23) 

Relating  (3.1)  and  (4.23),  we  can  obtain 

vrw  =  vLw  +/uwxa  (4.24) 

where  lLR  =  Ll-Lr.  Substituting  (4.12)  and  (4.13)  into  (4.24),  we  have, 
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SR\(d4SR23  aiCR2  ) 

CR\(  ^4SR23  ^  a2CR2  ) 
0 

SLl(^4SL2?>  ~  a2CL2  ) 

”  CL\(^4SL23  ~  a2CL2  ) 

0 


—  Cr\( d4CR23  +  C12Sr2  ) 

—  sRX(  d4cR23  +  a2sR  2  ) 

“  d<\SR23  &2CR2  ) 

~  CL\( d^ci23  fl2‘5>L2  ^ 

~  SL\(dACLTi  +  ^^2  ) 

~  d^SLrh  +  <22C£,2  ) 


^4CfliC/?23  ®/ei 

—  d4sRlcR23  Qr2  — 

'~ddSRr>^  Qr,. 


~d4cL]cL23  0L1  co„  ax 

—  d4sLXcL23  Ql2  +  lLR  ®0  x  ay 

~d4sL23  0L3  0  a 


(4.25) 


This  vector  equation  can  be  rewritten  in  three  scalar  equations  with  three 
unknowns  0^2,  con,  and  coo .  From  the  x  component  of  (4.25),  we  get: 

1  .  . 

Wo  ~  1  n  ^ Sr1  ^4SR23  ~~  a2CR2)®R\  “  CR\ (d^CR23  ^  a2SR2^R2  ~  ^4C/?lC/?23^ R3 

lLRaz  (4.26) 

—  su(d4sL23  —  a2c L2)0 l\  cL\(d4cL23  +&2sl2^®l2  d4cLXcL23Q  L3] 


From  the  y  component  of  (4.25),  we  get: 

wn  ~  -j  ~^~cR\(d4sR23  ~ d2cR2)^ RX  +  sRi(d4cR23  +  cl2Sr2)^r2  +  d4sRlcR23QR3 

lLRaz  (4.27) 

+  ('Ll  (d4S  123  ~&2CL2^Ll  ~~  ^LI  (^4CL23  ^^2SL2^L2  ~  ^4^Z,1CL23®Z.3 ] 

From  the  z  component  of  (4.25),  we  get  the  passive  joint  rate: 

1  . 


d*SR23  +  a2CR2 


[d4sR23QR3 (  d4sL23  +  a2cL2)QL2  d4sL23QL3  +lLR&y(On 

(4.28) 


Substituting  (4.26)  and  (4.27)  into  (4.28),  0*2  can  then  be  expressed  in  term  of 
active  joint  rates  as: 

®/?2  ”  L\  +e2^L2  +e3®L3  e$R\  +  e5^ R3 

(4.29) 


where  ex  (aycLi  axsL\)(d4sL23  ci2cL2 ) 

Uz  V  U4*R2  ^  U2CR2) 

e2—  ^[aZ(d4SL22  ~  a2CL2^  ~  (aySL\  axCL\ )(^4CL23  _  ^2^L2  )] 

Uz  v  U4^R2  U2CR2> 

1 

^3  /  J  ,  \  [az^4,SL23  ”^4CL23(av5’Ll  “axCLl)] 

aZV  w4‘i/?2  "ra2C/?2/ 


^4  ~  (ayCR\  +  axSR\) 
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65  n  (-J  c  A- nr  \[<V4'S«23  ^ACR23(avSR\  fljC«l)] 
MA  UA^R2  "t"a2tR2^ 


Substituting  (4.29)  into  (4.26)  and  (4.27),  we  can  obtain  the  angular  velocity  w. 
Substituting  w  into  (4. 1 1),  we  can  get  the  hand  velocity  v. 

The  forward  velocity  analysis  of  the  platform  robot  is  to  find  the  platform  velocity 
given  the  joint  rates.  This  can  be  accomplished  by  relating  the  velocity  of  the  three  wrists. 
Similar  to  (4.23)  we  can  relate  the  velocity  of  two  wrists  from  (4.1 1)  as: 

viw  =vjw  +  wx(l.-l.)  (4.30) 

where  i  and  j  indicate  wrists  on  different  arms. 

Relating  vlw  and  v3w ,  vlw  and  v2w  in  (4.30)  respectively,  these  two  vector 
equations  can  be  rewritten  in  six  scalar  equations  with  six  unknowns 
012,  022 ,  032 ,  ®„,  coo,  and  coa.  Notice  that  relating  v2w  and  v3w  in  (4.30)  we  can 
get  a  third  but  redundant  equation.  Once  the  angular  velocity  is  solved,  the  platform 
velocity  can  be  obtain  from  (4. 1 1). 


4.4  Case  Studies 

MATLAB  programs  were  written  to  verify  rate  control  algorithm  of  linkage  and 
platform  robots.  The  motion  simulation  of  a  platform  robot  is  performed  in  the  first  case. 
The  robot’s  upper  and  lower  link  are  1.0  m  long  in  each  arm.  The  feet  on  the  platform  and 
base  are  each  on  an  equilateral  triangle.  Each  side  of  the  base  and  platform  triangles  are 
4.0  m  and  2.0  m  respectively. 


For  inverse  velocity,  the  platform  velocity  is  given  as 


v 

w 


=  [0,0,-l;0,0,0f .  The 


platform  center  is  located  at  [0,0, 1.2],  with  yaw,  pitch,  and  roll  angles  as  [0,0,10],  The 
joint  rate  calculated  by  equation  (4.10)  is  q  =  [0,1.2627;0, 1.2627 ;0,1.2627] ,  and  that  by 
equation  (4.14)  is  q=[0, -1.0460, 1.2627;0, 1.0460, 1.2627;0, -1.0460, 1.2627],  Active  joint 
rates  from  these  two  results  agree.  The  joint  rates  can  also  be  verified  by  physical 
interpretation.  Since  each  wrist  has  a  downward  motion  only,  the  base  is  not  rotating  and 
0„  should  be  zero  as  calculated. 


In  the  second  case,  a  linkage  robot  is  studied.  The  robot’s  upper  and  lower  links 
are  1.0  m  long  in  each  arm,  and  the  distance  from  the  tool  to  the  left  wrist  is  1.3  m. 
Distance  between  the  two  bases  and  between  the  two  wrists  are  both  0.6  m.  For  inverse 
velocity,  the  tool  velocity  is  given  as  [0,  1,  0;'0,  0].  The  tool  position  is  at  [1.1,  0,  1.3], 
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with  yaw,  pitch,  roll  angles  as  [0,  85,  0].  The  rate  calculated  by  equation  (4.22)  is 
q  =  [5.1268,  0,  0;  5.1268,  0],  and  the  rate  calculated  by  (4.14)  is 

q  =  [5.1268,  0,  0;  5.1268,  0,  0] .  Eliminating  the  passive  joint  rate  from  the  latter  method, 
the  two  methods  yield  identical  results.  The  results  can  also  be  verified  by  physical 
interpretation.  Since  the  tool  is  moving  in  the  transverse  direction  only,  only  0  ,  is  non 
zero. 


4.5  Discussion 

As  stated  in  section  4.3,  five  actuators  can  be  used  for  the  5  DOF  linkage  robot 
without  becoming  a  singular  configuration.  For  better  load  distribution,  the  right  shoulder 
can  also  become  active,  and  the  robot  becomes  redundant  actuation. 

Since  each  column  of  S  can  be  expressed  by  (4.10),  the  S  matrix  can  easily  be 
expanded  to  a  rank  of  6x5  with  six  active  joints.  S_1  will  then  be  replaced  by  the 
pseudoinverse.  Because  of  the  pseudoinverse,  the  redundancy  will  yield  a  minimum  norm 
of  the  wrist  force,  and  in  turn  a  minimum  norm  of  the  joint  torque. 

The  resolved  force  control  presented  in  this  chapter  can  be  extended  to  any  serial- 
parallel  manipulator.  For  example,  a  planar  version  of  three  articulated-arm  platform 
robot  [Kokkinis  and  Stoughton  1988]  and  a  planar  version  of  the  linkage  robot  [Wang 
and  You  1992]  can  be  easily  adapted  derived  using  wrenches.  Three  DOF  spatial  platform 
manipulators  [Yang  1995]  can  also  be  derived  this  way. 
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CHAPTER  FIVE 


THE  DYNAMIC  MODEL  OF  A  LINKAGE  ROBOT 

The  Lagrange  formulation  is  used  to  derive  the  dynamic  models  of  a  linkage  robot 
from  their  potential  and  kinematics  energy.  The  dynamic  model  of  the  linkage  robot  will 
be  developed  by  hypothetically  cutting  the  linkage  at  the  two  wrists  into  three  segments 
and  finding  joint  torques  affected  by  each  of  the  three  parts. 


5.1  The  Dynamic  Model  of  a  Serial  Robot 

The  equation  of  motion  of  a  serial  robot  can  be  expressed  as: 

L  =  X  Du  (4)4  j  +  X  X  C‘kj (4)4 k4j  +  hi  (4)  +  b,  (?)  , 

7=1  k=l  ;=1  W-U 

where  T,  is  the  joint  torque, 

4  the  joint  rate, 

4  the  joint  acceleration, 

Dy  the  coupled  inertia  between  Joints  i  and  j, 

C'kj  the  Coriolis  and  centrifugal  terms  at  Joint  i, 
the  gravitational  torque  at  Joint  i, 
bt  the  frictional  torque  at  Joint  i. 

Dy  of  a  multiple  link  system  can  be  derived  from  the  kinetic  energy;  /z,  can  be 

derived  from  the  potential  energy  and  the  Lagrangian  equation;  C'kJ  can  be  derived  from 
D,j  using  the  Lagrangian  method. 


The  kinetic  energy  T  of  a  serial  robot  with  n  links  can  be  expressed  as: 

1  n  Y  n 

%?)  =  iI<Ikwk+-XXvk 

Z  k=\  * 


(5.2) 


where  Ik  the  inertial  tensor  of  Link  k, 
mk  the  mass  of  Link  k, 
wk  the  angular  velocity  of  Link  k, 
vk  the  centroidal  velocity  of  Link  k. 

wk  and  vk  are  functions  of  joint  rates  and  angles,  and  can  be  expressed  as: 
wk  ~  Akq  ( 

vk  =Bkq 
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where  AK  and  Bk  are  the  upper  and  lower  parts  of  the  manipulator  Jacobian  and  are 
associated  with  translation  and  rotation  respectively.  Substituting  (5.3)  into  (5.2),  we  can 
get 


T(q,q)  =  ^qT^[(\k)TmkAk  +(Bk)rl)kBk)ii  =  l-qr'Dq 


k= 1 


(5.4) 


where  Dk  is  the  kth  link’s  inertial  tensor,  and  D  is  the  manipulator  inertial  tensor. 
The  potential  energy  U  can  be  expressed  as: 

£/(«)  =  1>,*V 

*  =  1 


(5.5) 


where  mk  is  the  mass  of  link  k,  and  c*  is  the  position  of  the  center  of  mass  of  link  k  in 
the  base  coordinates. 

The  Lagrangian  L  is  defined  as  the  difference  between  the  kinetic  and  potential 
energy  of  a  mechanical  system. 


L(q,q)  =  T(q,q)-U(q) 


(5.6) 


The  equation  of  motion  of  a  serial  robotic  arm  can  be  formulated  in  terms  of  the 
Lagrangian  function  as  follows: 
ddL  3L 

'V  (5-7> 


Substituting  (5.4),  (5.5),  and  (5.6)  into  (5.7),  we  get: 

n  n  n 

L  =  X  Difij  +  X  X  CkMi  +  hi 


j= i 


k  =  l  j= 1 


(5.8) 


Gravitational  term  hi  can  expressed  as: 

n 

h,  =  -£oXm/; 


(5.9) 


where  g0  is  the  gravitational  constant,  and  cj  is  the  position  of  the  center  of  mass  of  the 
jth  link. 


The  Coriolis  and  centrifugal  terms  C'kj  can  be  related  to  Dr  as: 
13 D„ 


Cl  = 


dqk  2  dqt 


(5.10) 


5.2  The  Dynamic  Model  of  a  Linkage  Robot 
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The  dynamic  model  of  a  linkage  robot  can  be  analyzed  by  hypothetically  cutting 
the  linkage  at  the  two  wrists  into  three  segments.  The  joint  torque  can  then  be  obtained 
as: 

x,.=xf+x‘+xf +£>.  (5.11) 

where  bt  is  the  joint  friction;  xf,  X%  and  xf  are  joint  torques  affected  by  the  left  arm, 
coupler  arm, ,  right  arm  respectively. 

The  left  arm  can  be  analyzed  like  a  serial  robot.  The  right  arm  is  also  a  serial  arm, 
but  with  a  passive  shoulder  joint.  Therefore  dynamic  terms  associated  with  the  passive 
joint  rate  need  to  be  related  to  active  joint  rates.  Similarly,  to  find  dynamic  terms 
associated  with  the  coupler  link,  the  coupler  velocity  should  be  expressed  in  terms  of 
active  joint  rates. 


The  inertia  tensor  of  the  left  arm  can  be  expressed  as: 

Dn  0  0  1 

DL  =  0  Z)22  D2l3  (5.13) 

o  dl  nL 

U  U23  U 33  _ 

where  Dn  —  r2  m2cL2  +  ? icli  m4(r2sL23  ~~ a2cL2 )  ^slh 

D22  ~  ^2  ^2  1 2  ^4  (^*4  ~~  2r4^2 S  ^  +  Cl2  )  +  /4 

D23  =  W4(r42  -  r4a2SLi)  +  /4 
Dl  -  r2  +  1 

^33  '4  TJ4 


Once  DtJ  is  calculated,  the  corresponding  Qf  of  the  left  arm  can  be  derived  by 
(5.10).  The  gravitational  terms  are: 

/if  =0 

K  =  SolnhWu  +™A(a2cL2  -  r4st23)] 
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(5.14) 

(5.15) 


K  =  ~g0m4r4sL23 


(5.16) 


Substituting  (5.13),  (5.14),  (5.15),  and  (5.16)  into  (5.8),  the  joint  torques 
contributed  by  the  left  arm  are: 

Tj  =  +  C12  ^#2  +  Q3  4l#3 

^2  —  ^22*?2  ^23<?3  ^  ^1 1  Q\  +  ^32  #2*7 3  +  ^33  # 3  +  ^2  (5.17) 

^3  =  ^32#2  ^33#3  Ql  #1  +  ^22  #3  ^  ^C32  #2#3  +  ^3 

where  C12  —  — 2(r2“/iz2  -l"72)c^24S'L2  +  27724(r4sL23  —  ^"^2^12)  ^ ni> 

Cl3  —  2m4r4  (r4sL23  —  <z2cL2  )cL23  H-  2/4sL23cL23 

Cn  —  (r2  wz2  +  ^2  )^L2^z,2  ^4  (*4  ^L23  ^2CZ,2  Xr4  CL23  ^2^L2  )  "t"  ^4  SL2lCL22 

C3f  =  -2  m4r4a2c3 
C3l32  =  -m4r4a2c3 

C\\  —  2ttl4  (r4 SL23  —  a2CL2  )r4CL23  4S L23C L23 

C22  =  — m4r4a2c  L3 


5.4  Dynamic  Terms  Contributed  by  the  Coupler  Link 


The  coupler  arm’s  velocity  wc  and  vc  can  be  expressed  in  terms  of  active  joint 
rate  as  (3.20): 

V"l  =  (Sjrlq5 


w„ 


(5.18) 


where  S5  is  defined  in  (3.19).  From  (5.18),  wc  and  vc  can  be  expressed  as: 

j  0  0  0  ol 


vc  = 


wC  = 


0  10  0  0 
0  0  10  0 
'0  0  0  1  0 
0  0  0  0  1 


(Sj)-'q5 

(Sj)-‘q5 


(5.19) 

(5.20) 


Then  the  inertia  tensor  can  be  derived  from  the  kinetic  energy  as:. 
Tc  =^vc™cvc  +^wcIcwc  =^q!Dcq5 

Substituting  (5.19)  and  (5.20)  into  (5.21),  we  have: 


(5.21) 
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-1 


~mc 

0 

0 

0 

o' 

'0 

0 

0 

0 

o' 

0 

mc 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

mc 

0 

0 

(s5rr 

+  (S5)-' 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

K 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

h. 

dc  =  (S5) 


=  (S5)-,B(S5t)-1 

mc  0  0  0  0 

0  mc  0  0  0 

where  B  =  I  0  0  mc  0  0 

0  0  0  Ic  0 

0  0  0  0  / 


(S5r)- 


(5.22) 


The  velocity  coupling  term 

Mi 

product  rule  to  (5.22)  as: 


in  (5.10)  can  be  obtained  by  applying  the 


Tx-l 


j(Ss)  B(Sr}-i  +(gs)-iBa(Ss) 


Mi  dq, 


Mi 


(5.23) 


T)-l 


_  asj1  a(sj) 

To  get  and 


Mi 


^ —  in  this  equation,  we  can  use  the  identity  matrix 


I5  =  SjS;1 .  Taking  derivative  of  I5  with  respect  to  q, ,  we  have: 


Mi  Mi 


Mi 


Therefore,  ^^-  =  -Ss1-^-S<!1 

Mi  Mi 

Similarly,  using  I5  =  (Sj  )(Sj  )_1 ,  we  can  derive 

=  -(sT5y 


(5.24) 


Msjr'  _  ,cr,_,  as5r 


Mi 


Mi 


(sir'. 


(5.25) 


Since  matrix  inversion  is  computationally  intensive,  we  would  like  to  relate 
(Sj )_I  in  (5.25)  to  (Sj1  )T .  We  can  use  the  identity  matrix  Is  =  (Sj  )_1Sj ,  (5.26) 

and  I5  =  (S5SsI)t  (5.27) 
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Applying  the  transpose  identity  to  (5.27),  we  get  Is  =  (S“l)TSj , 


(5.28) 


Comparing  (5.26)  and  (5.28),  we  get  (Sj )_1  =  (S^)1 


Substituting  (5.29)  into  (5.25),  we  have 

f  \T 


^(S5  )  _  ,n-l,r  3S>5  |  r_ 

~  v®  5  '  V®  5  >  ~ 

Mt  M> 


®5  ® 5 


V 


Mi 


j 


Substituting  (5.24)  into  (5.30),  we  have 


9(Ss) 

Mi 


T\~  1  ( 


as 


-iV 


Mi 


Equation  (5.23)  can  then  be  written  as: 

dDr 


Mi  M 


as-1 

5  B(S5')r+S:'B 


a(sj‘) 

Mi 


(5.29) 


(5.30) 


(5.31) 


(5.32) 


Substituting  (5.32)  into  (5.10),  C'kj  can  then  be  obtained. 


To  get  the  gravitational  term,  the  coupler's  centroid  coordinates  can  be  derived  as: 


Pc=Plw+-^(Prw-Plw) 


(5.33) 


where  k  is  the  distance  between  the  two  wrists,  /CLis  the  distance  from  the  mass  center  of 
coupler  arm  to  the  left  wrist,  and  pLw  and  pRw  are  the  coordinates  of  the  left  and  right 
wrists  respectively. 


The  z  component  of  the  pc  can  be  expressed  as: 

1 

Pd  ~  £  [(cL  +  k)(a2SL2  "*”^4CL23  ~  ^CL^R2SR2  +  ^R3SR23 )]  (5-34) 


Therefore  the  gravitational  terms  can  be  derived  as: 

/rc  =  g()mc 

Mi 

Then  the  joint  torque  contributed  by  the  coupler  arm  is: 
<=t,  +XXC*'(«>‘Mj  +hf{q) 

7=1  *= 1  7=1 


(5.35) 


(5.36) 
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5.5  Dynamic  Terms  Contributed  by  the  right  arm 


The  right  arm  of  the  linkage  robot  has  a  similar  structure  as  the  left  arm.  So  the 
kinetic  energy  for  the  right  arm  can  be  written  as: 

y  _  QrD  Qr 

2  (5.37) 

=  Ai4ri  +  DiiQri  +  D33qR3  +  2D23qR2qR3 


From  (4.29)  qR2  can  be  expressed  in  term  of  active  joint  rates  as: 


T  •  •  T 

e  q  =  q  e 


(5.38) 


Tr  can  then  be  written  as: 

Tr  =  +  D22qTeeTq  +  D33q2R3  +  2D23eTq4/?3 


(5.39) 


qR 3  can  be  expressed  in  vector  form  and  then  (5.39)  can  be  rewritten  as: 
Tr  =  DxlqRl+D22 q  ee  q  +  D33^R3 +2D23q  e[o  0  0  0  l]q 


(5.40) 


Collecting  terms,  the  kinetic  energy  can  be  expressed  as: 
Tr=  qTDRq 

~Dn  0  0  0  O' 

0  0  0  0  0 

where  DR  =  0  0  D33  0  0  +D22eeT 

0  0  0  0  0 

0  0  0  0  2D23e5 


With  inertia  term  DR  ,  C'kj  can  then  be  derived  using  (5.10). 


(5.41) 


(5.42) 


Joint  torques  contributed  by  the  right  arm  can  be  expressed  as: 

T.*  =]LD*(4)4,  +XXcf  +/l/!(?) 


(5.43) 


Substituting  (5.17),  (5.36),  and  (5.43)  into  (5.1 1),  we  can  get  the  joint  torque  t. 


5.6  Discussion 

The  contribution  of  this  chapter  is  to  develop  a  dynamic  model  that  all  dynamic 
terms  can  be  explicitly  shown.  With  these  terms  we  can  evaluate  the  numerical  value  of 
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each  term  with  a  given  robot  geometry  and  under  a  specific  path.  If  certain  terms  like  Cy 
are  insignificant,  we  can  neglect  them  in  the  torque  calculation. 

The  challenge  of  the  linkage  robot  dynamic  model  developed  in  this  chapter  is  the 
calculation  of  (5.23)  in  the  coupler  arm  and  the  calculation  of  (5.42)  in  the  right  arm. 
Each  column  of  S5  in  (5.23)  is  a  sinusoidal  function  of  joint  angles,  and  the  derivatives  of 
S5  will  be  complex.  The  elements  of  vector  e  in  (5.37)  are  also  sinusoidal  functions  as 
shown  in  (4.29),  and  therefore  the  computation  is  time  consuming. 
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CHAPTER  SIX 


COLLISION  DETECTION  OF  A  LINKAGE  ROBOT 

An  efficient  collision  detection  algorithm  is  developed  for  linkage  and  platform 
robots.  To  detect  the  collision  of  multiple  arms  of  a  serial-parallel  manipulator,  the 
collision  of  any  two  links  of  the  articulated  arms  need  to  be  investigated  first.  Cylinders 
are  used  to  model  links,  and  line  equations  with  parameters  are  used  to  represent  center- 
lines  of  these  cylinders.  If  the  distance  between  two  center-lines  is  less  than  the  sum  of 
radii  of  these  two  cylinders,  and  the  feet  of  the  common  normal  of  the  two  center-lines 
are  inside  the  links,  collision  happens.  A  MATLAB  program  was  written  to  visualize  and 
verify  the  collision  detecting  algorithm. 


6.1  Collision  between  Two  Links 

Two  links  are  modeled  as  cylinders  and  are  showed  in  Figure  6.1  a.  Figure  6.1  b 
shows  two  line  segments  representing  the  cylinders'  center-lines.  P„,  PI2,  P21 ,  and  P22 
are  respective  endpoints  of  the  line  segments  L,  and  L2 .  The  relationship  between  the 
two  lines  containing  L,  and  L2  can  be  skew  or  coplanar,  and  the  coplanar  lines  can  be 
parallel,  intersecting,  and  collinear. 


(si =1 ) 


Pl2 


(82=1) 


(a) 


(b) 


Figure  6.  1  Two  links  and  their  common  normal 
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If  the  two  lines  containing  line  segments  L,  and  L2  are  skew,  there  is  a  unique 
common  normal  L  to  these  two  lines.  Osgood  and  Graustein  [1960]  used  direction 
components  of  the  two  lines  to  decide  their  common  normal,  and  calculated  the  distance 
between  the  two  lines  with  the  coordinates  of  one  point  on  each  line.  In  a  robot's  motion 
control,  the  coordinates  of  endpoints  of  each  link  (line  segment)  can  always  be  calculated 
from  joint  angles.  Therefore  coordinates  of  P„,  P12,  P21 ,  and  P22 ,  instead  of  direction 
components,  will  be  used  in  the  algorithm  derivation. 


Since  we  are  concerned  about  the  collision  of  links  (line  segments),  not  just  two 
lines,  the  location  of  the  feet  of  the  common  normal,  in  addition  to  the  distance  between 
them,  is  important.  To  decide  if  a  common  normal’s  foot,  P,  or  P2,  is  within  a  line 
segment,  they  can  be  represented  by  a  parametric  form  as: 

P1  —  P11  ^  ^1  (P12  —  )  (6.1) 

P2  =PM  +J2(Pj2  -p2i)  (6.2) 

where  and  s2  are  the  parameters.  When  0  <  s,  <  1 ,  a  normal  foot  is  within  the  line 
segment  of  Lr  The  normal  condition  between  the  common  normal  L  (P, -P2)  and  the 
two  line  segments  L,  ( P„  -  P12 )  and  L2  ( P21  -  P22 )  can  be  expressed  by  inner  products  as: 
(P2  —  Pi )  *  (I*i2  —  Pn )  =  0  (6.3) 

(P2-pi)*(P22-P2i)=:0  (6.4) 


Substituting  (6.1)  and  (6.2)  into  (6.3)  and  (6.4)  respectively,  we  get: 

((P2.  -  PU  )  +  ^2  0*22  -  P21 )  -  0*12  -  PU  ))  *  (P12  ~  P„  )  =  0 

((P21  -  Pl,  )  +  *2  0*22  -  P21 )  -  (P12  "  P11  ))  *  (P22  "  P21 )  =  0 

These  two  equations  can  be  expressed  in  terms  of  parameters  5,  and  s2  as: 
-a]  5,  +bs2  =  c, 

-bsx  +  a2s2  =  c2 

where  a,  =  (P12  -  Pu )  •  (P12  -  Pu ) 

a2  =  (P22  —  P21  )  *  (P22  —  P21 ) 
b  =(P22-P2,).(P12-Pn) 

c,=-(P2J-Pu).(P12-Pn) 

C2  =  (P21  —  P11  )  *  (P12  —  P11  ) 


(6.5) 

(6.6) 

(6.7) 

(6.8) 


When  (6.7)  and  (6.8)  are  linearly  independent,  we  can  solve  for  s,  and  s2  as: 

Si  =  (c,a2  “  c2b )  /  D  (6.9) 

s2  =  (~c2ax  +  c{b)  /  D  (6.10) 

where  D  =  -axa2+b 2 


If  both  s{  and  s2  are  within  limits,  the  two  cylinders  collide  when  the  distance 
between  the  two  feet  is  less  than  the  sum  of  link  diameters: 
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(6.11) 


\P2-Pl\<0.5(d2+dl) 

where  d2  and  are  respective  diameters  of  the  two  cylinders. 

The  link  collision  may  happen  when  a  foot  of  the  common  normal  is  outside  a 
link,  and  the  end  face  of  one  link  collides  the  other  link.  Figure  6.2  shows  the  end-view  of 
link  2,  with  the  center-line  of  link  1  in  a  plane  which  is  normal  to  link  2.  As  shown  in 
Figure  6.2b,  the  foot  of  the  common  normal  is  outside  the  cylinder  with  a  distance  of 

0.5d2  sin  0 ,  and  this  distance  is  largest  when  0  =  90° ,  as  shown  in  Figure  6.2c.  To 
account  for  this  type  of  collision,  the  range  criterion  is  modified  to: 

CLj  <Sj<\  +  aj  j=l,2  (6.12) 

where  a 7  is  the  ratio  of  the  radius  of  one  link  to  the  length  of  the  other  link:  a,  =  d2  /  2/, , 
and  a2  =  dx  /  2 12 . 


(a)  (b)  (c) 


Figure  6.  2  Cases  when  a  foot  of  the  common  normal  is  located  outside  the  link 


Equation  (6.3)  and  (6.4)  are  not  linearly  independent  if  one  of  the  following  two 
conditions  is  true. 

P2-Pj=0  (6.13) 

P22  —  P21  =  ^(Pj2  —  P«  )  (6. 14) 

If  (6.13)  is  true,  P2  =  P, ,  and  the  common  normal  is  degenerated  to  a  point.  This 
is  when  lines  containing  L,  and  are  intersecting,  and  equations  (6.3)  and  (6.4) 
become  trivial.  If  (6.14)  is  true,  the  two  line  segments  L,  and  are  in  the  same 
direction,  and  this  yields  D=0  in  (6.9)  and  (6.10).  If  the  two  lines  are  collinear,  there  is  no 
common  normal.  If  they  are  parallel,  there  are  an  infinite  number  of  common  normal 
lines. 

Equation  (6.13)  or  (6.14)  is  true  when  L,  and  are  coplanar.  The  coplanar 
condition  can  be  expressed  as: 

(pi2  -  P« )  •  [(P22  -  P21 )  x  (P2,  -  P„ )]  =  0  (6.15) 
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In  this  equation,  [(P22  -  P21 )  x  (P21  -  P„ )]  is  a  line  normal  to  4  and  a  line  (P21  -Pn) 
connecting  endpoints  of  L,  and  .  When  this  normal  line  is  perpendicular  to  L, ,  L, 
and  Lj  arecoplanar. 

When  lines  containing  L,  and  L,  are  coplanar  and  (6.14)  is  false,  they  are 
intersecting.  The  collision  detection  algorithm  should  be  modified  to  detect  the  case  when 
an  end  of  a  link  hits  another  link.  This  type  of  collision,  as  shown  in  Figure  6.3  can  be 
detected  by  monitoring  the  distance  between  a  point  P2  and  a  line  segment  L, ,  as  shown 
in  Figure  6.4. 


(b) 


Figure  6.  3  Collisions  of  links  in  planar  motion 
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(S1=1) 
/  Pl2 


Figure  6.  4  The  normal  line  between  a  point  and  a  line 


P,,  the  foot  of  the  normal  line  from  P2,  can  be  expressed  as  a  function  of  a 
parameter  s{ ,  as  in  (6.1).  Substituting  (6.1)  into  (6.3),  we  get: 

((P2  -  Pn )  -  5,  (P12  -  Pu ))  •  (P12  -  Pu )  =  0  (6. 16) 

Point  P2  here  can  be  one  of  the  endpoints  of  L, ,  P21  or  P22 .  Solving  (6.16)  for  5, ,  we 
get: 

5,  =  ((P2  -  Pu )  •  (P12  -  Pu ))  /  ((P12  -  Pn ))  •  (P12  -  P„  ))  .(6- 17) 

When  0  <  sl  <  1 ,  the  foot  is  within  the  line  segment.  The  collision  happens  if  the  length  of 
the  normal  line  is  short,  as  in  (6.1 1).  Similarly,  to  check  if  an  endpoint  of  L,  collides  with 
Lj ,  we  can  substitute  (6.2)  into  (6.4)  to  get: 

*2  =  ((Pi  "  Pn  )  *  (P22  -  P2i ))  /  ((P22  -  P2, ))  •  (P22  "  P2i ))  (6. 1 8) 

When  0  <  s2  <  1 ,  the  foot  is  within  the  line  segment.  The  collision  happens  if  the  length 
of  the  normal  line  is  short,  as  in  (6.1 1). 

L,  and  are  collinear  when  (6.14)  is  true  and  a  line  (P21  -  Pn) ,  which  connects 
endpoints  of  L,  and  L, ,  is  also  in  the  same  direction  as  L,  (P22  -  P21) . 

P22  -  P21  =  k( P21  -  Pu )  where  k  is  a  constant  (6. 19) 

To  check  link  collision,  we  need  to  find  if  the  endpoints  of  L, ,  Pn  or  P12 ,  are  on 
Lj .  Substituting  these  two  points  successively  as  P2  in  (2),  we  can  solve  for  s2 . 

s2=(P12-P2i)/(P22-P21)  (6.20) 

or  s2=(Pn-P2,)/(P22-P2,)  (6.21) 
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If  0<s2  <1,  collision  happens.  When  (6.13)  is  true  and  (6.19)  is  false,  L,  and  L,  are 
parallel.  Solving  (6.7),  we  can  express  s2  in  term  of  5,  as: 

s2  =  (c,  -axsx)lb  (6.22) 

For  every  s{  in  (6.22),  there  is  an  s2.  Thus,  there  are  an  infinite  number  of  common 
normal  lines.  For  link  collision,  we  need  to  check  the  common  normal  at  the  endpoints. 
Therefore  we  should  use  s,  =  0  or  s,  =  1  in  (6.22)  to  check  if  s2  is  within  the  range.  The 
whole  collision  detection  algorithm  is  summarized  in  Figure  6.5  for  cases  when  the  two 
lines  containing  Ly  and  are  collinear,  parallel,  intersecting,  or  skew. 


Skew  Intersecting  Collinear  Parallel 


Figure  6.  5  The  collision  detection  algorithm 

6.2.  Collision  Detection  of  a  Linkage  Robot 

The  linkage  robot,  as  shown  in  Figure  2.1,  has  6  links  and  Cg=15  pairs  of  links. 
To  investigate  its  collision  detection,  one  pair  of  links  is  investigated  at  a  time.  Among 
the  15  pairs,  six  pairs  consist  of  adjacent  links,  and  they  can  be  avoided  by  joint  stops. 
Therefore,  the  possible  collision  of  the  remaining  nine  pairs  should  be  investigated  using 
the  algorithm  in  Figure  6.5. 

A  MATLAB  program  was  developed  to  execute  the  collision  detection.  Figure  6.6 
shows  a  sample  case  of  the  program.  The  ‘Simulate’  button  is  to  start  the  collision 
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detection,  and  the  ‘Close’  button  is  to  terminate  the  simulation.  ‘Start  at’,  ‘End  at’, 
‘Approach’,  ‘View’  are  editable  buttons.  The  tool  trajectory  can  be  modified  by  editing 
‘Start  at’  and  ‘End  at’  buttons.  The  tool  orientation  is  decided  by  editing  the  ‘Approach’ 
button’.  The  ‘View’  button  is  to  change  the  view  direction.  The  ‘Info’  button  is  to  explain 
this  program. 


Figure  6.  6  A  collision  detection  program 


6.3  Discussion 

The  collision  detection  algorithm  of  serial-parallel  robots  can  be  easily  modified  to 
detect  collisions  of  two  coordinating  robots.  There  are  three  moving  links  in  each  robot, 
and  a  total  of  six  moving  links  need  to  be  considered.  Because  the  algorithm  is  efficient, 
collision  detection  of  multiple  robots  can  be  accomplished  effectively.  To  detect  collision 
of  three  robots  sharing  the  common  workspace,  the  collision  between  two  robots  will  be 
checked  first,  and  then  the  algorithm  will  be  applied  to  the  other  two  pairs  of  robots. 
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CHAPTER  SEVEN 


SUMARY  AND  DISCUSSION 

A  linkage  robot  is  built,  analyzed,  and  graphically  simulated  in  this  research.  The 
position,  velocity,  and  force  analysis  of  the  linkage  robot  was  derived  in  this  project.  The 
force  and  velocity  analysis  is  based  on  the  wrench  of  the  screw  theory,  and  this  can  be 
easily  extended  to  other  serial-parallel  manipulators  like  a  three-articulated-arm  robot. 

A  prototype  linkage  robot  was  built  at  a  cost  of  over  $5,000.  A  teach  pendant  can 
control  the  motion  of  the  robot,  but  the  Rhino’s  controller  has  difficulties  accepting  the 
user’s  own  control  programs  because  the  download  software  has  errors. 

MATLAB  programs  were  written  to  verify  the  rate  control  algorithms,  to  simulate 
motion  of  continuous  paths,  and  to  detect  possible  collisions.  MATLAB’s  strength  is 
matrix  manipulation,  and  it  is  ideal  for  this  robotic  control. 

MATLAB  graphic  simulation  is  limited  to  a  stick  model  of  robots,  and  sometimes 
is  confusing  because  of  viewing  directions.  For  future  simulation  work,  Working  Model 
3D  [Knowledge  Revolution  1996]  should  be  considered  because  of  its  ease  of  use  and  its 
real  time  link  with  MATLAB.  Working  Model  3D  can  also  provide  built-in  collision 
detection. 
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'String',  textEStr); 

btnPos=[xPos  yPos-spacing  btnWid  btnHt/2]; 
EeditHndl=uicontrol( ... 

'Style', 'edit', ... 

'Units', 'normalized', ... 
'Position',btnPos2, ... 

'String', editEStr); 


%===========================— 

%  The  View  Direction  button 
%==================================== 

btnNumber=4; 

yPos=0.90-(btnNumber- 1  )*(btnHt+spacing); 

textVStr='View'; 

editVStr='-3  -4  5'; 

%  View  button  information 
btnPosl=[xPos  yPos-spacing+btnHt/2  btnWid  btnHt/2]; 
btnPos2=[xPos  yPos-spacing  btnWid  btnHt/2]; 
VeditHndl=uicontrol( ... 

'Style', 'text', ... 

'Units'/normalized', ... 

'Position’,btnPosl, ... 

'String', textVStr); 

btnPos=[xPos  yPos-spacing  btnWid  btnHt/2]; 
VeditHndl=uicontrol( ... 

'Style', 'edit', ... 

'Units', 'normalized', ... 

'Position’,btnPos2, ... 

’String',editVStr); 


%================================ 

%  The  Approach  direction  button 
%=============================== 

btnNumber=5; 

yPos=0.90-(btnNumber-l)*(btnHt+spacing); 
text  AStr=' Approach' ; 
editAStr='l  0  O’; 

%  Approach  direction  button  information 
btnPosl=[xPos  yPos-spacing+btnHt/2  btnWid  btnHt/2]; 
btnPos2=[xPos  yPos-spacing  btnWid  btnHt/2]; 
AeditHndl=uicontrol( ... 
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'Style', 'text', ... 

'Units', 'normalized', ... 
'Position',btnPosl, ... 

'String',  textAStr); 

btnPos=[xPos  yPos-spacing  btnWid  btnHt/2]; 
AeditHndl=uicontrol( ... 

'Style', 'edit', ... 

’Units’, 'normalized', ... 

'Position', btnPos2, ... 

'String',  editAStr); 


%  The  INFO  button 

%==============——=—== 

labelStr='Info'; 
callbackStr='lrobot("info")'; 
infoHndl=uicontrol( ... 

'Style', 'push', ... 

'Units', 'normalized', ... 

'Position', [xPos  0.20  btnWid  0.10], ... 
’String’, labelStr, ... 

’Callback',  callbackStr); 


%============ 

%  The  CLOSE  button 

labelStr='Close'; 
callbacks  tr='close(gcf)'; 
closeHndl=uicontrol( ... 

'Style', 'push', ... 

'Units', 'normalized', ... 

'Position', [xPos  0.05  btnWid  0.10], ... 
'String', labelStr, ... 

'Callback', callbackStr); 


%===================—=====_= 

%  Set  the  figure  visible  after  all  buttons  are  defined. 
%==================================== 

hndlList=[startHndl  SeditHndl  EeditHndl  VeditHndl  AeditHndl  infoHndl  closeHndl]; 
set(figNumber, ... 

'Visible', 'on', ... 

'U  serData'.hndlList) ; 
watchoff(oldFigNumber); 
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figure(figNumber); 


elseif  strcmp(action, 'simulate'), 
axHndl=gca; 
figNumber=gcf; 

hndlList=get(figNumber,'Userdata'); 
startHndl=hndlList(  1 ) ; 

SeditHndl=hndlList(2) ; 

EeditHndl=hndlList(3); 

V  editHndl=hndlList(4) ; 

AeditHndl=hndlList(5); 
infoHndl=hndlList(6); 
closeHndl=hndlList(7) ; 

set([startHndl  closeHndl  infoHndl  SeditHndl  EeditHndl  VeditHndl 
AeditHndl], 'Enable', 'off); 
set(axHndl,'Userdata',play); 


%  =====  Start  of  Simulation 

%  This  is  the  main  program  for  the  Linkage  Robot  Simulation  Problem 
%  This  function  makes  use  of  the  following  other  functions: 

%  inverse9.m(A  user  defined  program)  and  helpfun.m(A  standard  MATLAB 
function) 

%  ===== 


%  Change  view  direction 
VStr=get(VeditHndl, 'String'); 
vd=sscanf(VStr,'%f); 
view(vd); 

format  long; 

%  Read  in  data  of  initial  position,  final  position,  and  approach  vector. 
psStr=get(SeditHndl, 'String'); 
ps=sscanf(psStr,'%f); 
peStr=get(EeditHndl, 'String'); 
pe=sscanf(peStr,'%f); 
aStr=get(  AeditHndl, 'String'); 
a=sscanf(aStr,'%f); 

%  parameters  of  the  linkage  robot 
dl=l  .3;  %  the  distance  from  the  left  wrist  to  hand 

d2=.7;  %  the  distance  from  the  right  wrist  to  hand 
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12=1 ;  %  lower  link  length 

13=1 ;  %  upper  link  length 

h=.6;  %  the  distance  between  the  left  and  right  shoulder 

nl=10;  %  the  number  of  intervals  of  the  straight  line  path 

%  Mesh  setting  of  the  simulation  background 

X=-l:.2:2;  %minimum:  interval:  maximum 

Y=-.8:.2:.8; 

[XX,YY]=meshgrid(X,Y); 

ZZ=XX*0+YY*0;  %mesh  plane  is  the 

zgrid=0:.2:2; 

num=size(zgrid); 

xl=-l; 

yl=.8; 

x2=2; 

y2=.8; 

x  1  s=zgrid*0+x  1 ;  %mesh  coordinate  for  vertical  line 

yls=zgrid*0+yl; 
x2s=zgrid*0+x2; 
y2s=zgrid*0+y2; 
cla; 

set(figNumber,... 

'Visible', 'on',... 

'U  serData',hndlList) ; 

plot3(XX,YY,ZZ,'c.');  %”c”,  cyan,  each  data  point  represented  by  a  dot 
hold  on  %upgrading  figure  without  erasing 

plot3(x  1  s,y  1  s,zgrid,'c.'); 
hold  on 

plot3(x2s,y2s,zgrid,'c.'); 
axis  off 

axis([-1.2  2.2  -1.0  1.0-1  2]); 

hold  on 

set(axHndl,... 

'Nextplot','add') 
set(axHndl, 'Drawmode', 'Fast') 
drawnow 
hold  on 

%  Looping  for  the  simulation 
for  k=l:nl+l 
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if  k>l  %  moving  robot 

set(joHandle,'XData',d(:,l),'YData’,d(:,2),'ZData',d(:,3));  %Joint  position 

set(li  1  Handle, 'XData',d(  1 :2,  l),'YData',d(  1 :2,2),'ZData',d(  1 :2,3))  %Link  segment  from 
set(li2Handle,'XData’,d(2:3,l),,Ydata',d(2:3,2),'ZData',d(2:3,3))  %point  1  to  point  2 
set(li3Handle,'XData,,d(3:4,l),'Ydata',d(3:4,2),'ZData’,d(3:4,3)) 
set(li4Handle,'XData',d(5:6,l),'Ydata',d(5:6,2),'ZData,,d(5:6,3)) 
set(li5Handle,'XData',d(6:7,l),'Ydata',d(6:7,2),'ZData,,d(6:7,3)) 
set(statusHandle, 'String', st);  %”set”,  modify  the  figure  according  to  new  data 
drawnow 
end 

%  node  position 
p=ps+(k- 1 )  *  (pe-ps)/n  1 ; 
pl=p-a*dl; 
p2=p-a*d2-[h;0;0]; 

[cs,st]=inverse9(pl,p2,12,13);  %  function  to  calculate  the  joint  angles 

if  st=='out  of  workspace' 
break 
end 


%  left  arm  joint  position 

%  plO,  pi  1,  pl2,  pl3  are  base,  elbow,  wrist,  and  tool  position 

P10=[0;0;0]; 

p  1 1 =[12*cs(  1  )*cs(3);12*cs(2)*cs(3);12*cs(4)]; 

pl2=[cs(l)*(12*cs(3)-13*cs(6));cs(2)*(12*cs(3)-13*cs(6));12*cs(4)+13*cs(5)]; 

pl3=p; 

%  check  the  link  length  and  coupler  length 

%  lll=sqrt((pl  l-plO)'*(pl  1-plO))  %  11 1  is  the  left  lower  arm  length 

%  Iul=sqrt((pl2-pll)'*(pl2-pll))  %  112  is  the  left  upper  arm  length 

%  Icl=sqrt((pl3-pl2)'*(pl3-pl2));  %  lcl  is  the  coupler  arm  length 

%  right  arm  position 

p20=[h;0;0]; 

p21=p20+[12*cs(7)*cs(9);12*cs(8)*cs(9);12*cs(10)]; 

p22=p20+[cs(7)*(12*cs(9)-13*cs(12»;cs(8)*(12*cs(9)-13*cs(12));12*cs(10)+13*cs(l  1)]; 
p23=p22+a*d2; 

112=sqrt((p2 1  -p20)'*(p2 1  -p20»;  % 

Iu2=sqrt((p22-p2 1  )'*(p22-p2 1 )); 

Ia2=sqrt((p23-p22)'*(p23-p22)); 

Ia3=sqrt((p22-p  1 2)'*(p22-p  1 2)); 

d=[p  1 0'  ;p  1 1'  ;p  1 2'  ;p23';p22'  ;p2 1 '  ;p20'] ; 
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* 


■t 


if  k==l  %  draw  the  initial  position 

joHandle=plot3(d(:,l),d(:, 2), d(:, 3), 'ro','EraseMode', 'background'); 

%  “ro”  draw  the  line  in  red  color  and  “o”  sign  at  end  points 

HlHandle=plot3(d(  l:2,l),d(  1:2, 2), d(l:2, 3), 'y-’,'EraseMode', 'background'); 

%  “y-“  draw  the  line  in  yellow  color  and  continue  line 

li2Handle=plot3(d(2:3,l),d(2:3, 2), d(2:3,3),'g-’,'EraseMode', 'background');  “g”,  green 
li3Handle=plot3(d(3:4,l),d(3:4,2),d(3:4,3),'c-','EraseMode','background'); 
li4Handle=plot3(d(5:6,l),d(5:6,2),d(5:6,3),'m-','EraseMode','background');“m”,magenta 
li5Handle=plot3(d(6:7,l),d(6:7, 2), d(6:7, 3), 'w-','EraseMode', 'background');  “w”,  white 
statusHandle=text( . .  . 

'HorizontalAlignmentVleft',... 

'Vertical  Alignment' ,  'bottom', . . . 

'Position',[-0.2,0,-2.03]„.. 

'EraseMode','background',... 

'String', st); 

axis  off 
drawnow 
end 

%  reduce  the  animation  speed  for  observation 
%  for  jj=  1:5000, 

%  jjj=i+jj; 

%  end 

end  %  end  for  the  "elseif  strcmp(action, 'simulate')" 

%  activate  the  control  button  after  the  simulation  done 
set([startHndl  closeHndl  infoHndl  SeditHndl  EeditHndl  VeditHndl 
AeditHndl], 'Enable', 'on'); 

elseif  strcmp(action,'info’); 
inStr='Linkage  Robot  Simulation  Information'; 
hlpStr= ... 

[' 

'  This  program  animates  the  straight  line  motion  of  a  linkage 
'  robot.  Clicking  the  animate  button  starts  the  animation  with 
'  the  default  values  of  the  program.  ' 

'  t 

'  Use  the  "Start  at",  "End  at",  and  "Approach"  buttons  to  edit  ' 

'  the  start,  final  position  and  the  approach  vector.  ' 

'  » 

'  "View"  button  is  to  edit  the  view  direction.  ' 

*  i 
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'  This  program  makes  use  of  two  other  functions  inverse.m: 

'  and  helpfun.m.  ']; 

helpfun(inStr,hlpStr); 

end;  %for  if  strcmp(action) 


function  [cs,st]=inverse9(pl,pr, 12,13); 
format  long 

%  INVERSE:  This  function  calculates  the  six  joint  angles  of  the  linkage  robot,  pi  and 
%  pr  are  the  position  vector  of  the  left  and  right  wrists.  12  and  13 

%  are  the  link  lengths  of  the  lower  and  upper  arms,  [cs]  is  the  output 

%  vector  of  sin  and  cos  functions  of  the  six  angles. 

%  Angles  of  left  arm 

thl  1  =atan2(pl(2),pl(  1 ));  %theta  1 

thl3s=-((pr*pl-12A2-13A2)/2*12*13); 

thl3c=sqrt(  1  -thl3sA2); 

thl3=atan2(thl3s,thl3c);  %  theta  3 

sll=sin(thll); 

cll=cos(thll); 

sl3=sin(thl3); 

cl3=cos(thl3); 

yy=pl(3)*(12-13*sl3)-13*cl3*(pl(l)*cll+pl(2)*sll); 
xx=pl(3)*13*cl3+(12-13*sl3)*(pl(l)*cll+pl(2)*sll); 
thl2=atan2(yy,xx);  %theta  2 


%  Angles  of  right  arm 

thr  1  =atan(pr(2)/pr(  1 )) ; 

thr3s=-((pr'*pr-12A2-13A2)/2*12*13); 

thr3c=sqrt(  l-thr3sA2); 

thr3=atan2(thr3s,thr3c); 

srl=sin(thrl); 

crl=cos(thrl); 

sr3=sin(thr3); 

cr3=cos(thr3); 

yy=pr(3)*(12-13*sr3)-13*cr3*(pr(l)*crl+pr(2)*srl); 
xx=pr(3)  *  13  *  cr3+(12-13  *  sr3)  *  (pr(  1  )*cr  1  +pr(2)*sr  1 ); 
thr2=atan2(yy,xx); 
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sll=sin(thll); 

cll=cos(thll); 

sl2=sin(thl2); 

cl2=cos(thl2); 

srl=sin(thrl); 

crl=cos(thrl); 

sr2=sin(thr2); 

cr2=cos(thr2); 

sl23=sin(thl2+thl3); 

cl23=cos(thl2+thl3); 

sr23=sin(thr2+thr3 ) ; 

cr23=cos(thr2+thr3) ; 

cs=[cll,sll,cl2,sl2,cl23,sl23,crl,srl,cr2,sr2,cr23,sr23]; 

if  thr3sA2>l  lthl3sA2>l, 
st='out  of  workspace’ 
else 
st='  ’ 
end 
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